/***********************************************************************************************************
 * 
 ***********************************************************************************************************/
inline float3 RBMath::Cross(const float3& a, const float3& b)
{
    float3 c;
    c.x = a.y * b.z - a.z * b.y;
    c.y = a.z * b.x - a.x * b.z;
    c.z = a.x * b.y - a.y * b.x;
    return c;
}

/***********************************************************************************************************
 *
 ***********************************************************************************************************/
inline float4x4 RBMath::Build(const float3& Translate, const float3x3& Rotate, const float3& Scale)
{
    float4x4 out;
    out(0,0) = Scale.x * Rotate(0,0); out(0,1) = Scale.y * Rotate(0,1); out(0,2) = Scale.z * Rotate(0,2); out(0,3) = Translate.x;
    out(1,0) = Scale.x * Rotate(1,0); out(1,1) = Scale.y * Rotate(1,1); out(1,2) = Scale.z * Rotate(1,2); out(1,3) = Translate.y;
    out(2,0) = Scale.x * Rotate(2,0); out(2,1) = Scale.y * Rotate(2,1); out(2,2) = Scale.z * Rotate(2,2); out(2,3) = Translate.z;
    out(3,0) = 0                    ; out(3,1) = 0                    ; out(3,2) = 0                    ; out(3,3) = 1;
    return out;
}

/***********************************************************************************************************
 *
 ***********************************************************************************************************/
inline float4x4 RBMath::Build(const RBTransform& trans)
{
    return Build(trans.Translate, trans.Rotate, trans.Scale);
}

/**************************************************************************************************************************************************************
 * 
 **************************************************************************************************************************************************************/
template<uint8 dim, class T> T RBMath::MultVector(const T& left, const T& right)
{
    T out;
    for (uint8 ui=0; ui<dim; ++ui)
    {
        out(ui) = left(ui) * right(ui);
    }
    return out;
}

/**************************************************************************************************************************************************************
 * 
 **************************************************************************************************************************************************************/
template<uint8 dim, class T> T RBMath::MultMatrix(const T& left, const T& right)
{
    T out;
    for (uint8 uiRow=0; uiRow<dim; ++uiRow)
    {
        for (uint8 uiCol=0; uiCol<dim; ++uiCol)
        {
            out(uiRow, uiCol) = 0;
            for (uint8 ui=0; ui<dim; ++ui)
            {
                out(uiRow, uiCol) += left(uiRow,ui) * right(ui, uiCol);
            }
        }
    }
    return out;
}


/**************************************************************************************************************************************************************
 * 
 **************************************************************************************************************************************************************/
template<uint8 dim, class T> T RBMath::AddMatrix(const T& left, const T& right)
{
    T out;
    for (uint8 uiRow=0; uiRow<dim; ++uiRow)
    {
        for (uint8 uiCol=0; uiCol<dim; ++uiCol)
        {
            out(uiRow, uiCol) = left(uiRow,uiCol) + right(uiRow, uiCol);
        }
    }
    return out;

}

/**************************************************************************************************************************************************************
 * 
 **************************************************************************************************************************************************************/
template<uint8 dim, class T> T RBMath::SubMatrix(const T& left, const T& right)
{
    T out;
    for (uint8 uiRow=0; uiRow<dim; ++uiRow)
    {
        for (uint8 uiCol=0; uiCol<dim; ++uiCol)
        {
            out(uiRow, uiCol) = left(uiRow,uiCol) - right(uiRow, uiCol);
        }
    }
    return out;
}

/**************************************************************************************************************************************************************
* 
**************************************************************************************************************************************************************/
template<uint8 dim, class T> T RBMath::ScaleMatrix(const T& mat, float fScale)
{
    T out;
    for (uint8 uiRow=0; uiRow<dim; ++uiRow)
    {
        for (uint8 uiCol=0; uiCol<dim; ++uiCol)
        {
            out(uiRow, uiCol) = fScale * mat(uiRow, uiCol);
        }
    }
    return out;
}

/**************************************************************************************************************************************************************
 * 
 **************************************************************************************************************************************************************/
template<uint8 dim, class T> T RBMath::TransposeMatrix(const T& mat)
{
    T out;
    for (uint8 uiRow=0; uiRow<dim; ++uiRow)
    {
        for (uint8 uiCol=0; uiCol<dim; ++uiCol)
        {
            out(uiRow, uiCol) = mat(uiCol, uiRow);
        }
    }
    return out;

}

/**************************************************************************************************************************************************************
 * 
 **************************************************************************************************************************************************************/
template<uint8 dim, class Matrix, class Vector> Vector RBMath::Transform(const Matrix& mat, const Vector& point)
{
    Vector out;

    for (uint8 uiRow=0; uiRow<dim; ++uiRow)
    {
        out(uiRow) = 0;
        for (uint8 uiCol=0; uiCol<dim; ++uiCol)
        {
            out(uiRow) += mat(uiRow, uiCol) * point(uiCol);
        }
    }

    return out;

}

/**************************************************************************************************************************************************************
 * 
 **************************************************************************************************************************************************************/
template<uint8 dim, class Matrix, class Vector> void RBMath::GetCol(const Matrix& mat, Vector& point, uint8 uiCol)
{
    for (uint8 uiRow = 0; uiRow<dim; ++uiRow)
    {
        point(uiRow) = mat(uiRow, uiCol);
    }
}

/**************************************************************************************************************************************************************
 * 
 **************************************************************************************************************************************************************/
template<uint8 dim, class Matrix, class Vector> void RBMath::SetCol(Matrix& mat, const Vector& point, uint8 uiCol)
{
    for (uint8 uiRow = 0; uiRow<dim; ++uiRow)
    {
        mat(uiRow, uiCol) = point(uiRow);
    }
}

/**************************************************************************************************************************************************************
 * 
 **************************************************************************************************************************************************************/
template<uint8 dim, class Matrix, class Vector> void RBMath::GetRow(const Matrix& mat, Vector& point, uint8 uiRow)
{
    for (uint8 uiCol = 0; uiCol<dim; ++uiCol)
    {
        point(uiRow) = mat(uiRow, uiCol);
    }
}

/**************************************************************************************************************************************************************
 * 
 **************************************************************************************************************************************************************/
template<uint8 dim, class Matrix, class Vector> void RBMath::SetRow(Matrix& mat, const Vector& point, uint8 uiRow)
{
    for (uint8 uiCol = 0; uiCol<dim; ++uiCol)
    {
        mat(uiRow, uiCol) = point(uiRow);
    }
}


inline float4x4 RBMath::InvertMatrix( float4x4 mat )
{
    float32 a0 = mat( 0)*mat( 5) - mat( 1)*mat( 4);
    float32 a1 = mat( 0)*mat( 6) - mat( 2)*mat( 4);
    float32 a2 = mat( 0)*mat( 7) - mat( 3)*mat( 4);
    float32 a3 = mat( 1)*mat( 6) - mat( 2)*mat( 5);
    float32 a4 = mat( 1)*mat( 7) - mat( 3)*mat( 5);
    float32 a5 = mat( 2)*mat( 7) - mat( 3)*mat( 6);
    float32 b0 = mat( 8)*mat(13) - mat( 9)*mat(12);
    float32 b1 = mat( 8)*mat(14) - mat(10)*mat(12);
    float32 b2 = mat( 8)*mat(15) - mat(11)*mat(12);
    float32 b3 = mat( 9)*mat(14) - mat(10)*mat(13);
    float32 b4 = mat( 9)*mat(15) - mat(11)*mat(13);
    float32 b5 = mat(10)*mat(15) - mat(11)*mat(14);

    float32 det = a0*b5 - a1*b4 + a2*b3 + a3*b2 - a4*b1 + a5*b0;
    if (RBMath::Abs(det) > RBMath::RB_EPSILON)
    {
        float4x4 inverse;
        inverse( 0) = + mat( 5)*b5 - mat( 6)*b4 + mat( 7)*b3;
        inverse( 4) = - mat( 4)*b5 + mat( 6)*b2 - mat( 7)*b1;
        inverse( 8) = + mat( 4)*b4 - mat( 5)*b2 + mat( 7)*b0;
        inverse(12) = - mat( 4)*b3 + mat( 5)*b1 - mat( 6)*b0;
        inverse( 1) = - mat( 1)*b5 + mat( 2)*b4 - mat( 3)*b3;
        inverse( 5) = + mat( 0)*b5 - mat( 2)*b2 + mat( 3)*b1;
        inverse( 9) = - mat( 0)*b4 + mat( 1)*b2 - mat( 3)*b0;
        inverse(13) = + mat( 0)*b3 - mat( 1)*b1 + mat( 2)*b0;
        inverse( 2) = + mat(13)*a5 - mat(14)*a4 + mat(15)*a3;
        inverse( 6) = - mat(12)*a5 + mat(14)*a2 - mat(15)*a1;
        inverse(10) = + mat(12)*a4 - mat(13)*a2 + mat(15)*a0;
        inverse(14) = - mat(12)*a3 + mat(13)*a1 - mat(14)*a0;
        inverse( 3) = - mat( 9)*a5 + mat(10)*a4 - mat(11)*a3;
        inverse( 7) = + mat( 8)*a5 - mat(10)*a2 + mat(11)*a1;
        inverse(11) = - mat( 8)*a4 + mat( 9)*a2 - mat(11)*a0;
        inverse(15) = + mat( 8)*a3 - mat( 9)*a1 + mat(10)*a0;

        float32 invDet = ((float32)1)/det;
        inverse( 0) *= invDet;
        inverse( 1) *= invDet;
        inverse( 2) *= invDet;
        inverse( 3) *= invDet;
        inverse( 4) *= invDet;
        inverse( 5) *= invDet;
        inverse( 6) *= invDet;
        inverse( 7) *= invDet;
        inverse( 8) *= invDet;
        inverse( 9) *= invDet;
        inverse(10) *= invDet;
        inverse(11) *= invDet;
        inverse(12) *= invDet;
        inverse(13) *= invDet;
        inverse(14) *= invDet;
        inverse(15) *= invDet;

        return inverse;
    }

    return float4x4::Identity();
}